#ifndef __UDSF_DBSCAN_C_
#define __UDSF_DBSCAN_C_

#include "udf_global.h"
#include "udsf_cstdlike.c"
#include "udsf_spi.c"
#include "udsf_uart.c"
/*
*   notice:
*   UDSF 同UDF编程：不可调用cstd-lib,只可调用"sdk_ifs_udk_cfg.h"、"sys_api.h"中列出的API函数。
*   UDSF文件内函数均需使用static修饰符，且UDF中以#include "UDSF.c"形式引用
*/

#define MAX_PCR_PER_CLUSTER (32)

#if defined(DBSCAN_SP_ENABLE)
// 单点 无速度维dbscan
static void udsf_dbscan_sp(STRUCT_POINTRST *frameRst,point_t *rst,struct dbscan_sp *pDBSCAN,int corept_index) __attribute__((unused));

static void udsf_dbscan_sp(STRUCT_POINTRST *frameRst,point_t *rst,struct dbscan_sp *pDBSCAN,int corept_index)
{

	pDBSCAN->cfg.epsRIdx      = paramRSV->dbscan_epsRIdx;
	pDBSCAN->cfg.epsVIdx      = paramRSV->dbscan_epsVIdx;
	pDBSCAN->cfg.epsSinPhiIdx = paramRSV->dbscan_epsSinPhiIdx;
	
	pDBSCAN->divFac = &paramRSV->divFac_temp.value[0];
	
	uint8_t * tmp_flag  = pDBSCAN->rsv;
	uint8_t * tmp_index = pDBSCAN->rsv + frameRst->targetNum;
	point_t * tmp_point = (point_t*)(pDBSCAN->rsv + frameRst->targetNum*2);
	
	udsf_memset(tmp_flag,0,frameRst->targetNum); // 仅clear tmp_flag

	for( int i=0;i<frameRst->targetNum;i++){
		tmp_point[i].d1Idx = frameRst->target[i].d1Idx;
		if( frameRst->target[i].d3Idx >= NUM_ANGLE/2 ){
			tmp_point[i].d3Idx = frameRst->target[i].d3Idx - NUM_ANGLE;
		}else{
			tmp_point[i].d3Idx = frameRst->target[i].d3Idx;
		}
	}
	tmp_flag[corept_index] = 1;
	tmp_index[corept_index] = corept_index;

	int circle_end = corept_index+1;
	int circle_cnt = 0;

	int cnt = 0;
	for( int j=corept_index;j<circle_end;j++){
		for(int i=0;i<frameRst->targetNum;i++){
			if( tmp_flag[i] != 1 ){
				int value = (tmp_point[tmp_index[j]].d1Idx - tmp_point[i].d1Idx>0)?(tmp_point[tmp_index[j]].d1Idx - tmp_point[i].d1Idx):(tmp_point[i].d1Idx - tmp_point[tmp_index[j]].d1Idx);
				if( value <  pDBSCAN->cfg.epsRIdx ){
					value = (tmp_point[tmp_index[j]].d3Idx - tmp_point[i].d3Idx>0)?(tmp_point[tmp_index[j]].d3Idx - tmp_point[i].d3Idx):(tmp_point[i].d3Idx - tmp_point[tmp_index[j]].d3Idx);
					if( value < pDBSCAN->cfg.epsVIdx ){
						if( circle_cnt == 0 ){
							tmp_index[cnt] = i;
							cnt++;
						}
						tmp_flag[i] = 1;
					}
				}
				tmp_flag[i] = 1;
			}
		}
		if( j+1 >= circle_end ){
			circle_cnt++;
			if( circle_cnt < 2 ){ // 开启第二次循环，查找并标记临点的临点
				j=0;
				circle_end = cnt;
			}else{ // 二次循环结束,退出
				break;
			}
		}
	}
	
	int d1Idx_Mean=0;
	int d3Idx_Mean=0;

	cnt = 0;
	for( int i=0;i<frameRst->targetNum;i++ ){
		if( tmp_flag[i] == 1 ){
			d1Idx_Mean += tmp_point[i].d1Idx;
			d3Idx_Mean += tmp_point[i].d3Idx;
			cnt ++;
			if( cnt >= MAX_PCR_PER_CLUSTER ){
				break; // 点数太多强制结束
			}
		}
	}
	
	pDBSCAN->dbscanClu.numCluster = 1;
	pDBSCAN->dbscanClu.cluster[0].vIdxMean = 0;
	pDBSCAN->dbscanClu.cluster[0].rIdxMean = d1Idx_Mean * pDBSCAN->divFac[cnt - 1];
	pDBSCAN->dbscanClu.cluster[0].sinPhiIdxMean = d3Idx_Mean * pDBSCAN->divFac[cnt - 1];

	rst->d1Idx = (pDBSCAN->dbscanClu.cluster[0].rIdxMean)>>12;
	rst->d3Idx = (pDBSCAN->dbscanClu.cluster[0].sinPhiIdxMean)>>12;
}
#endif

#if defined(DBSCAN_ENABLE)
static void udsf_dbscan(struct dbscan *pDBSCAN,uint16_t* divFac,DBSCANCFG*dbscan_cfg,STRUCT_POINTRST *frameRst,uint8_t vel_num) __attribute__((unused));

static void udsf_dbscan(struct dbscan *pDBSCAN,uint16_t* divFac,DBSCANCFG*dbscan_cfg,STRUCT_POINTRST *frameRst,uint8_t vel_num)
{
	for(int i=0;i<sizeof(struct dbscan);i++){
		((uint8_t*)pDBSCAN)[i] = 0;
	}
		
	pDBSCAN->divFac = divFac;
	// dbscan config
	pDBSCAN->cfg.minPts       = dbscan_cfg->minPts;// paramRSV->dbscan_minPts;
	pDBSCAN->cfg.epsRIdx      = dbscan_cfg->epsRIdx;
	pDBSCAN->cfg.epsVIdx      = dbscan_cfg->epsVIdx;
	pDBSCAN->cfg.epsSinPhiIdx = dbscan_cfg->epsSinPhiIdx;

	frameRst->targetNum > FRAME_MAX_NUM ? (pDBSCAN->dbscanFrame.ptNum = FRAME_MAX_NUM) : (pDBSCAN->dbscanFrame.ptNum = frameRst->targetNum);
	
	for (int i = 0; i < pDBSCAN->dbscanFrame.ptNum; i++)
	{
		pDBSCAN->dbscanFrame.pt[i].rIdx = frameRst->target[i].d1Idx;
		if (frameRst->target[i].d2Idx >= vel_num/2)
			pDBSCAN->dbscanFrame.pt[i].vIdx = frameRst->target[i].d2Idx - vel_num;
		else
			pDBSCAN->dbscanFrame.pt[i].vIdx = frameRst->target[i].d2Idx;
		if (frameRst->target[i].d3Idx >= NUM_ANGLE/2 )
			pDBSCAN->dbscanFrame.pt[i].sinPhiIdx = frameRst->target[i].d3Idx - NUM_ANGLE;
		else
			pDBSCAN->dbscanFrame.pt[i].sinPhiIdx = frameRst->target[i].d3Idx;
		pDBSCAN->dbscanFrame.pt[i].abs = frameRst->target[i].powABS;
	}

	int len = pDBSCAN->dbscanFrame.ptNum;

	// dbscan init
	int clusterID = 0;
	int isNB = 0;

	for (int i = 0; i < len; i++)
	{
		pDBSCAN->dbscanFrame.pt[i].visited   = 0;
		pDBSCAN->dbscanFrame.pt[i].corePt    = 0;
		pDBSCAN->dbscanFrame.pt[i].clusterID = UDF_DBSCAN_IDLE;
	}
	pDBSCAN->nbA.ptNum = 0;
	pDBSCAN->nbB.ptNum = 0;
					
	// dbscan
	for (int i = 0; i < len; i++)
	{
		if (pDBSCAN->dbscanFrame.pt[i].visited == 0)
		{
			pDBSCAN->dbscanFrame.pt[i].visited = 1;

			// DBSCAN_regionQuery Start
			int neighborPts = 0;
			pDBSCAN->nbA.ptNum = 0;
			for (int j = 0; j < len; j++)
			{
				//DBSCAN_isNB Start
				isNB = 0;
				int absVal = pDBSCAN->dbscanFrame.pt[i].rIdx - pDBSCAN->dbscanFrame.pt[j].rIdx > 0 ? (pDBSCAN->dbscanFrame.pt[i].rIdx - pDBSCAN->dbscanFrame.pt[j].rIdx) : (pDBSCAN->dbscanFrame.pt[j].rIdx - pDBSCAN->dbscanFrame.pt[i].rIdx);
				if (absVal < pDBSCAN->cfg.epsRIdx)
				{
					absVal = pDBSCAN->dbscanFrame.pt[i].vIdx - pDBSCAN->dbscanFrame.pt[j].vIdx > 0 ? (pDBSCAN->dbscanFrame.pt[i].vIdx - pDBSCAN->dbscanFrame.pt[j].vIdx) : (pDBSCAN->dbscanFrame.pt[j].vIdx - pDBSCAN->dbscanFrame.pt[i].vIdx);
					if (absVal < pDBSCAN->cfg.epsVIdx)
					{
						absVal = pDBSCAN->dbscanFrame.pt[i].sinPhiIdx - pDBSCAN->dbscanFrame.pt[j].sinPhiIdx > 0 ? (pDBSCAN->dbscanFrame.pt[i].sinPhiIdx - pDBSCAN->dbscanFrame.pt[j].sinPhiIdx) : (pDBSCAN->dbscanFrame.pt[j].sinPhiIdx - pDBSCAN->dbscanFrame.pt[i].sinPhiIdx);
						if (absVal < pDBSCAN->cfg.epsSinPhiIdx)
						{
							isNB = 1;
						}
					}
				}
				//DBSCAN_isNB End
				if (isNB)
				{
					neighborPts++;
					pDBSCAN->nbA.idx[pDBSCAN->nbA.ptNum] = j;
					pDBSCAN->nbA.ptNum++;
				}
			}
			// DBSCAN_regionQuery End

			if (neighborPts < pDBSCAN->cfg.minPts)
			{
				pDBSCAN->dbscanFrame.pt[i].clusterID = UDF_DBSCAN_NOISE;
			}
			else
			{
				clusterID++;
				pDBSCAN->dbscanFrame.pt[i].corePt    = 1;
				pDBSCAN->dbscanFrame.pt[i].clusterID = (int8_t)clusterID;
				for (int j = 0; j < pDBSCAN->nbA.ptNum; j++)
				{
					if (pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].visited == 0)
					{
						pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].visited = 1;

						// DBSCAN_regionQuery Start
						neighborPts = 0;
						pDBSCAN->nbB.ptNum = 0;
						for (int k = 0; k < len; k++)
						{
							//DBSCAN_isNB Start
							isNB = 0;
							int absVal = pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].rIdx - pDBSCAN->dbscanFrame.pt[k].rIdx > 0 ? (pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].rIdx - pDBSCAN->dbscanFrame.pt[k].rIdx) : (pDBSCAN->dbscanFrame.pt[k].rIdx - pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].rIdx);
							if (absVal < pDBSCAN->cfg.epsRIdx)
							{
								absVal = pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].vIdx - pDBSCAN->dbscanFrame.pt[k].vIdx > 0 ? (pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].vIdx - pDBSCAN->dbscanFrame.pt[k].vIdx) : (pDBSCAN->dbscanFrame.pt[k].vIdx - pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].vIdx);
								if (absVal < pDBSCAN->cfg.epsVIdx)
								{
									absVal = pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].sinPhiIdx - pDBSCAN->dbscanFrame.pt[k].sinPhiIdx > 0 ? (pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].sinPhiIdx - pDBSCAN->dbscanFrame.pt[k].sinPhiIdx) : (pDBSCAN->dbscanFrame.pt[k].sinPhiIdx - pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].sinPhiIdx);
									if (absVal < pDBSCAN->cfg.epsSinPhiIdx)
									{
										isNB = 1;
									}
								}
							}
							//DBSCAN_isNB End
							if (isNB)
							{
								neighborPts++;
								pDBSCAN->nbB.idx[pDBSCAN->nbB.ptNum] = k;
								pDBSCAN->nbB.ptNum++;
							}
						}
						// DBSCAN_regionQuery End

						if (neighborPts >= pDBSCAN->cfg.minPts)
						{
							// DBSCAN_neighborUnion Start
							for (int m = 0; m < pDBSCAN->nbB.ptNum; m++)
							{
								int n;
								for ( n = 0; n < pDBSCAN->nbA.ptNum; n++)
								{
									if (pDBSCAN->nbB.idx[m] == pDBSCAN->nbA.idx[n])
										break;
								}
								if (n == pDBSCAN->nbA.ptNum)
								{
									pDBSCAN->nbA.idx[pDBSCAN->nbA.ptNum] = pDBSCAN->nbB.idx[m];
									pDBSCAN->nbA.ptNum++;
								}
							}
							// DBSCAN_neighborUnion End
						}
					}
					if (pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].clusterID == UDF_DBSCAN_IDLE)
					{
						pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].clusterID = (int8_t)clusterID;
					}
				}
			}
		}
	}

	/* dbscan_ClusterRpt */
	// find clusterID max
	int index = 0;
	for (int i = 1; i < len; i++)
	{
		if (pDBSCAN->dbscanFrame.pt[index].clusterID < pDBSCAN->dbscanFrame.pt[i].clusterID)
			index = i;
	}
	pDBSCAN->dbscanClu.numCluster = pDBSCAN->dbscanFrame.pt[index].clusterID;

	if (pDBSCAN->dbscanClu.numCluster == 0xFF){
		pDBSCAN->dbscanClu.numCluster = 0;
	}
	
	for (int i = 1; i <= pDBSCAN->dbscanClu.numCluster; i++)
	{
		int numPt           = 0;
		int rIdxMean        = 0;
		int vIdxMean        = 0;
		int sinPhiIdxMean   = 0;
		uint32_t abs_total  = 0;
		uint32_t abs_tmp    = 0;
		
		#if defined(NEAREST_MEAN)
		struct tmp{
			uint16_t value;
			uint16_t index;
		} nearest_buff[NEAREST_MEAN]={0};
		int cnt = 0;
		for( int i=0;i<NEAREST_MEAN;i++){
			nearest_buff[i].value = (uint16_t)(-1);
		}
		#endif		
		
		for (int j = 0; j < len; j++)
		{
			if (pDBSCAN->dbscanFrame.pt[j].clusterID == i)
			{
				if( numPt < MAX_PCR_PER_CLUSTER ){
					rIdxMean        = rIdxMean      + pDBSCAN->dbscanFrame.pt[j].rIdx;
					vIdxMean        = vIdxMean      + pDBSCAN->dbscanFrame.pt[j].vIdx;
					sinPhiIdxMean   = sinPhiIdxMean + pDBSCAN->dbscanFrame.pt[j].sinPhiIdx;
					abs_tmp = abs_total + pDBSCAN->dbscanFrame.pt[j].abs;
					if( abs_tmp > abs_total ){
						abs_total = abs_tmp;
					}
					numPt           = numPt + 1;
				}
				
				#if defined(NEAREST_MEAN)
				// 找最近点
				if( (pDBSCAN->dbscanFrame.pt[j].rIdx < nearest_buff[0].value) || (cnt < NEAREST_MEAN)){
					//淘汰末位
					nearest_buff[0].value = pDBSCAN->dbscanFrame.pt[j].rIdx;
					nearest_buff[0].index = j+1;
					

					#if ( NEAREST_MEAN > 1 )
					int max = 1;
					for( int i=2;i<NEAREST_MEAN;i++){
						if( nearest_buff[i].value > nearest_buff[i-1].value ){
							max = i;
						}
					}
					// 更新末位
					if( nearest_buff[0].value < nearest_buff[max].value ){
						struct tmp tmp = nearest_buff[0];
						nearest_buff[0] = nearest_buff[max];
						nearest_buff[max] = tmp;
					}
					#endif
					cnt++;
				}
				#endif
			}
		}
		
		// 距离最近的N个点云均值
		#if defined(NEAREST_MEAN)
		rIdxMean = 0;
		vIdxMean = 0;
		sinPhiIdxMean = 0;
		numPt = 0;
		for( int i=0;i<NEAREST_MEAN;i++){
			if( nearest_buff[i].index > 0 ){
				if( numPt < MAX_PCR_PER_CLUSTER ){
					rIdxMean += pDBSCAN->dbscanFrame.pt[nearest_buff[i].index-1].rIdx;
					vIdxMean += pDBSCAN->dbscanFrame.pt[nearest_buff[i].index-1].vIdx;
					sinPhiIdxMean += pDBSCAN->dbscanFrame.pt[nearest_buff[i].index-1].sinPhiIdx;
					numPt += 1;
				}
			}
		}
		#endif

		pDBSCAN->dbscanClu.cluster[i - 1].rIdxMean = rIdxMean * pDBSCAN->divFac[numPt - 1];
		pDBSCAN->dbscanClu.cluster[i - 1].vIdxMean = vIdxMean * pDBSCAN->divFac[numPt - 1];
		pDBSCAN->dbscanClu.cluster[i - 1].sinPhiIdxMean = sinPhiIdxMean * pDBSCAN->divFac[numPt - 1];
		pDBSCAN->dbscanClu.cluster[i - 1].abs      = abs_total;

		// sinPhiIdxMean = sinPhiIdxMean >> 12;

		// sinPhiIdxMean   < 0 ? (sinPhiIdxMean = sinPhiIdxMean + 64) : (sinPhiIdxMean = sinPhiIdxMean);
		// pDBSCAN->dbscanClu.cluster[i - 1].rMean          = (rIdxMean * divFacR[numPt - 1]);
		// pDBSCAN->dbscanClu.cluster[i - 1].vMean          = (vIdxMean * divFacV[numPt - 1]);
		// pDBSCAN->dbscanClu.cluster[i - 1].sinPhiMean     = (sinPhiFac[sinPhiIdxMean - 1]);
		// pDBSCAN->dbscanClu.cluster[i - 1].xMean          = (rIdxMean * divFacR[numPt - 1] * cosPhiFac[sinPhiIdxMean - 1]);
		// pDBSCAN->dbscanClu.cluster[i - 1].yMean          = (rIdxMean * divFacR[numPt - 1] * sinPhiFac[sinPhiIdxMean - 1]);
	}
}
#endif

#if defined(DBSCAN2_ENABLE)
static void udsf_dbscan2(struct dbscan2 *pDBSCAN,PARAM_RSV * paramRSV,STRUCT_POINTRST *frameRst,uint8_t vel_num) __attribute__((unused));

static void udsf_dbscan2(struct dbscan2 *pDBSCAN,PARAM_RSV * paramRSV,STRUCT_POINTRST *frameRst,uint8_t vel_num)
{
	
	for(int i=0;i<sizeof(struct dbscan2);i++){
		((uint8_t*)pDBSCAN)[i] = 0;
	}
	
	pDBSCAN->divFac = (uint16_t*)&paramRSV->divFac;
	
	/*config load*/
	pDBSCAN->cfg.minPts       = paramRSV->dbscan_minPts;
	pDBSCAN->cfg.epsRIdx      = paramRSV->dbscan_epsRIdx;
	pDBSCAN->cfg.epsVIdx      = paramRSV->dbscan_epsVIdx;
	pDBSCAN->cfg.epsSinPhiIdx = paramRSV->dbscan_epsSinPhiIdx;

	/*param init*/
	frameRst->targetNum > FRAME_MAX_NUM ? (pDBSCAN->dbscanFrame.ptNum = FRAME_MAX_NUM) : (pDBSCAN->dbscanFrame.ptNum = frameRst->targetNum);


	for( int step = 0; step < 2; step++ ){
		if( step == 0 ){
			for (int i = 0; i < pDBSCAN->dbscanFrame.ptNum; i++)
			{
				pDBSCAN->dbscanFrame.pt[i].rIdx = frameRst->target[i].d1Idx;
				if (frameRst->target[i].d2Idx >= vel_num/2)
					pDBSCAN->dbscanFrame.pt[i].vIdx = frameRst->target[i].d2Idx - vel_num;
				else
					pDBSCAN->dbscanFrame.pt[i].vIdx = frameRst->target[i].d2Idx;
				if (frameRst->target[i].d3Idx >= NUM_ANGLE/2 )
					pDBSCAN->dbscanFrame.pt[i].sinPhiIdx = frameRst->target[i].d3Idx - NUM_ANGLE;
				else
					pDBSCAN->dbscanFrame.pt[i].sinPhiIdx = frameRst->target[i].d3Idx;
				
				pDBSCAN->dbscanFrame.pt[i].abs = frameRst->target[i].powABS;
			}
		}else{
				pDBSCAN->dbscanFrame.ptNum = 0;
				
				for (int i = 0; i < paramRSV->dbscan_mean_buffer_num; i++)
				{
						if (pDBSCAN->clusterBuf.clu[i].numClu > CLUSTER_MAX_NUM){
								pDBSCAN->clusterBuf.clu[i].numClu = 0;
						}
						for (int j = 0; j < pDBSCAN->clusterBuf.clu[i].numClu; j++)
						{
								pDBSCAN->dbscanFrame.pt[j + pDBSCAN->dbscanFrame.ptNum].rIdx = pDBSCAN->clusterBuf.clu[i].cluBuf[j].rIdx;
								pDBSCAN->dbscanFrame.pt[j + pDBSCAN->dbscanFrame.ptNum].vIdx = pDBSCAN->clusterBuf.clu[i].cluBuf[j].vIdx;
								pDBSCAN->dbscanFrame.pt[j + pDBSCAN->dbscanFrame.ptNum].sinPhiIdx = pDBSCAN->clusterBuf.clu[i].cluBuf[j].sinPhiIdx;
						}
						pDBSCAN->dbscanFrame.ptNum += pDBSCAN->clusterBuf.clu[i].numClu;
				}
		}

		
		
		int len = pDBSCAN->dbscanFrame.ptNum;
		
		if( step == 1 ){
			pDBSCAN->cfg.minPts  = 1;
			pDBSCAN->cfg.epsSinPhiIdx = 64;
		}
		
		// dbscan init
		int clusterID = 0;
		int isNB = 0;

		for (int i = 0; i < len; i++)
		{
			pDBSCAN->dbscanFrame.pt[i].visited   = 0;
			pDBSCAN->dbscanFrame.pt[i].corePt    = 0;
			pDBSCAN->dbscanFrame.pt[i].clusterID = UDF_DBSCAN_IDLE;
		}
		
		pDBSCAN->nbA.ptNum = 0;
		pDBSCAN->nbB.ptNum = 0;
					
		// dbscan
		for (int i = 0; i < len; i++)
		{
			if (pDBSCAN->dbscanFrame.pt[i].visited == 0)
			{
				pDBSCAN->dbscanFrame.pt[i].visited = 1;

				// DBSCAN_regionQuery Start
				int neighborPts = 0;
				pDBSCAN->nbA.ptNum = 0;
				for (int j = 0; j < len; j++)
				{
					//DBSCAN_isNB Start
					isNB = 0;
					int absVal = pDBSCAN->dbscanFrame.pt[i].rIdx - pDBSCAN->dbscanFrame.pt[j].rIdx > 0 ? (pDBSCAN->dbscanFrame.pt[i].rIdx - pDBSCAN->dbscanFrame.pt[j].rIdx) : (pDBSCAN->dbscanFrame.pt[j].rIdx - pDBSCAN->dbscanFrame.pt[i].rIdx);
					if (absVal < pDBSCAN->cfg.epsRIdx)
					{
						absVal = pDBSCAN->dbscanFrame.pt[i].vIdx - pDBSCAN->dbscanFrame.pt[j].vIdx > 0 ? (pDBSCAN->dbscanFrame.pt[i].vIdx - pDBSCAN->dbscanFrame.pt[j].vIdx) : (pDBSCAN->dbscanFrame.pt[j].vIdx - pDBSCAN->dbscanFrame.pt[i].vIdx);
						if (absVal < pDBSCAN->cfg.epsVIdx)
						{
							absVal = pDBSCAN->dbscanFrame.pt[i].sinPhiIdx - pDBSCAN->dbscanFrame.pt[j].sinPhiIdx > 0 ? (pDBSCAN->dbscanFrame.pt[i].sinPhiIdx - pDBSCAN->dbscanFrame.pt[j].sinPhiIdx) : (pDBSCAN->dbscanFrame.pt[j].sinPhiIdx - pDBSCAN->dbscanFrame.pt[i].sinPhiIdx);
							if (absVal < pDBSCAN->cfg.epsSinPhiIdx)
							{
								isNB = 1;
							}
						}
					}
					//DBSCAN_isNB End
					if (isNB)
					{
						neighborPts++;
						pDBSCAN->nbA.idx[pDBSCAN->nbA.ptNum] = j;
						pDBSCAN->nbA.ptNum++;
					}
				}
				// DBSCAN_regionQuery End

				if (neighborPts < pDBSCAN->cfg.minPts)
				{
					pDBSCAN->dbscanFrame.pt[i].clusterID = UDF_DBSCAN_NOISE;
				}
				else
				{
					clusterID++;
					pDBSCAN->dbscanFrame.pt[i].corePt    = 1;
					pDBSCAN->dbscanFrame.pt[i].clusterID = (int8_t)clusterID;
					for (int j = 0; j < pDBSCAN->nbA.ptNum; j++)
					{
						if (pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].visited == 0)
						{
							pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].visited = 1;

							// DBSCAN_regionQuery Start
							neighborPts = 0;
							pDBSCAN->nbB.ptNum = 0;
							for (int k = 0; k < len; k++)
							{
								//DBSCAN_isNB Start
								isNB = 0;
								int absVal = pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].rIdx - pDBSCAN->dbscanFrame.pt[k].rIdx > 0 ? (pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].rIdx - pDBSCAN->dbscanFrame.pt[k].rIdx) : (pDBSCAN->dbscanFrame.pt[k].rIdx - pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].rIdx);
								if (absVal < pDBSCAN->cfg.epsRIdx)
								{
									absVal = pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].vIdx - pDBSCAN->dbscanFrame.pt[k].vIdx > 0 ? (pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].vIdx - pDBSCAN->dbscanFrame.pt[k].vIdx) : (pDBSCAN->dbscanFrame.pt[k].vIdx - pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].vIdx);
									if (absVal < pDBSCAN->cfg.epsVIdx)
									{
										absVal = pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].sinPhiIdx - pDBSCAN->dbscanFrame.pt[k].sinPhiIdx > 0 ? (pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].sinPhiIdx - pDBSCAN->dbscanFrame.pt[k].sinPhiIdx) : (pDBSCAN->dbscanFrame.pt[k].sinPhiIdx - pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].sinPhiIdx);
										if (absVal < pDBSCAN->cfg.epsSinPhiIdx)
										{
											isNB = 1;
										}
									}
								}
								//DBSCAN_isNB End
								if (isNB)
								{
									neighborPts++;
									pDBSCAN->nbB.idx[pDBSCAN->nbB.ptNum] = k;
									pDBSCAN->nbB.ptNum++;
								}
							}
							// DBSCAN_regionQuery End

							if (neighborPts >= pDBSCAN->cfg.minPts)
							{
								// DBSCAN_neighborUnion Start
								for (int m = 0; m < pDBSCAN->nbB.ptNum; m++)
								{
									int n;
									for ( n = 0; n < pDBSCAN->nbA.ptNum; n++)
									{
										if (pDBSCAN->nbB.idx[m] == pDBSCAN->nbA.idx[n])
											break;
									}
									if (n == pDBSCAN->nbA.ptNum)
									{
										pDBSCAN->nbA.idx[pDBSCAN->nbA.ptNum] = pDBSCAN->nbB.idx[m];
										pDBSCAN->nbA.ptNum++;
									}
								}
								// DBSCAN_neighborUnion End
							}
						}
						if (pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].clusterID == UDF_DBSCAN_IDLE)
						{
							pDBSCAN->dbscanFrame.pt[pDBSCAN->nbA.idx[j]].clusterID = (int8_t)clusterID;
						}
					}
				}
			}
		}

		/* dbscan_ClusterRpt */
		// find clusterID max
	
		if( step == 0 ){
			if( pDBSCAN->clusterBuf.loopIdx >= paramRSV->dbscan_mean_buffer_num )
			{
				pDBSCAN->clusterBuf.loopIdx = 0;
			}
		}
		
		int index = 0;
		for (int i = 1; i < len; i++)
		{
			if (pDBSCAN->dbscanFrame.pt[index].clusterID < pDBSCAN->dbscanFrame.pt[i].clusterID)
				index = i;
		}
		pDBSCAN->dbscanClu.numCluster = pDBSCAN->dbscanFrame.pt[index].clusterID;

		if (pDBSCAN->dbscanClu.numCluster == 0xFF){
			pDBSCAN->dbscanClu.numCluster = 0;
		}
		
		if( step == 0 ){
			pDBSCAN->clusterBuf.clu[pDBSCAN->clusterBuf.loopIdx].numClu = pDBSCAN->dbscanClu.numCluster;
		}
		
		for (int i = 1; i <= pDBSCAN->dbscanClu.numCluster; i++)
		{
			int numPt           = 0;
			int rIdxMean        = 0;
			int vIdxMean        = 0;
			int sinPhiIdxMean   = 0;
			uint32_t abs_total  = 0;
			uint32_t abs_tmp    = 0;
		
			for (int j = 0; j < len; j++)
			{
				if (pDBSCAN->dbscanFrame.pt[j].clusterID == i)
				{
					if( numPt < MAX_PCR_PER_CLUSTER ){
						rIdxMean        = rIdxMean      + pDBSCAN->dbscanFrame.pt[j].rIdx;
						vIdxMean        = vIdxMean      + pDBSCAN->dbscanFrame.pt[j].vIdx;
						sinPhiIdxMean   = sinPhiIdxMean + pDBSCAN->dbscanFrame.pt[j].sinPhiIdx;
						
						abs_tmp = abs_total + pDBSCAN->dbscanFrame.pt[j].abs;
						if( abs_tmp > abs_total ){
							abs_total = abs_tmp;
						}
						numPt           = numPt + 1;
					}
				}
			}

			pDBSCAN->dbscanClu.cluster[i - 1].rIdxMean = rIdxMean * pDBSCAN->divFac[numPt - 1];
			pDBSCAN->dbscanClu.cluster[i - 1].vIdxMean = vIdxMean * pDBSCAN->divFac[numPt - 1];
			pDBSCAN->dbscanClu.cluster[i - 1].sinPhiIdxMean = sinPhiIdxMean * pDBSCAN->divFac[numPt - 1];
			pDBSCAN->dbscanClu.cluster[i - 1].abs      = abs_total;
			if( step == 0 ){
				pDBSCAN->clusterBuf.clu[pDBSCAN->clusterBuf.loopIdx].cluBuf[i - 1].rIdx = pDBSCAN->dbscanClu.cluster[i - 1].rIdxMean >> 12;
				pDBSCAN->clusterBuf.clu[pDBSCAN->clusterBuf.loopIdx].cluBuf[i - 1].vIdx = pDBSCAN->dbscanClu.cluster[i - 1].vIdxMean >> 12;
				pDBSCAN->clusterBuf.clu[pDBSCAN->clusterBuf.loopIdx].cluBuf[i - 1].sinPhiIdx = pDBSCAN->dbscanClu.cluster[i - 1].sinPhiIdxMean >> 12;
			}
			if( step == 0 ){
				pDBSCAN->clusterBuf.loopIdx += 1;
			}
		}
	}
}
#endif

			
#endif
